#include <EEPROM.h>
#include <Driver.h>
//---------sensor------wireless-----output
byte     cthrot = 0,  wthrot = 0,   throt = 0;//Throtel (output is in function)
byte    cbrakes = 0, wbrakes = 0, brakes = 0;//Brakes (output is in function)
boolean clights = 0, wlights = 0,  lights = 0;//Headlights
boolean    cdir = 0,    wdir = 0,     dir = 0;//Direction
byte       cbat = 0,    wbat = 0,     bat = 0;//Batery charge
boolean    ccap = 0,    wcap = 0,     cap = 0;//capacitor discharge controls
//pin Vars
byte throti=0, brakesi=0, lightsi=0, diri=0, bati=0,  tcallow=EEPROM.read(0),  bcallow=EEPROM.read(2);//inputs
byte   capo=0,            lightso=0, diro=0, bato=0, tcalhigh=EEPROM.read(1), bcalhigh=EEPROM.read(3);//outputs
//other variables
boolean override = 0;
boolean radio;
byte ramprate;

void decode(byte dbyte, byte throt, byte batt, byte brake){//Decodes data packets
 wthrot = throt;
 wbat = batt;
 wbrakes = brake;
 override = 0;//set vars to standard
 wdir = 1;
 wlights = 0;
 wcap = 0;
  if (bitRead(dbyte,0) == 1)override = 1;//change as per Bit
  if (bitRead(dbyte,1) == 0)wdir = 0;
  if (bitRead(dbyte,2) == 1)wlights = 1;
  if (bitRead(dbyte,3) == 1)wcap = 1;
}
Driver motor(capo,diro,ramprate);


void setup() {
  Serial.begin(9600);  //set serial to 9600 baud rate
  pinMode(lightso, OUTPUT);
  pinMode(lightsi, INPUT);
  pinMode (diri, INPUT);
}

void loop(){
  //serial
radio = 0;
outscync:
  if (Serial.available() > 3) {
    if (Serial.read() == 115){
      Serial.print('S',BYTE);
      radio = 1;
      decode(Serial.read(),Serial.read(), Serial.read(), Serial.read());
}else{Serial.flush(); delay(1000); goto outscync;}}

  //inputs
  cthrot = analogRead(throti);
  cbrakes = analogRead(brakesi);
  clights = digitalRead(lightsi);
  cdir = digitalRead(diri);
  cbat = analogRead(bati);

  //processes
  if (override == 1){ // radio override or not????
   lights = wlights;
   bat = wbat;
   if (wdir != dir)
   motor.changedir(wdir);
   throt=wthrot;
   brakes=wbrakes;
  }else{
    lights = clights;
    bat = map(cbat,0,945,0,116);
   if (cdir != dir)
   motor.changedir(cdir);
throt = map(cthrot,tcallow,tcalhigh,0,255);
brakes = map(cbrakes,bcallow,bcalhigh,0,255);
  }
  //outputs
  analogWrite(bato,bat);
  digitalWrite(lightso,lights);
  digitalWrite(diro,dir);
  motor.setspeed(throt,brakes);
  int byteone;
  if(override) bitSet(byteone,0);
  if(cdir) bitSet(byteone,1);
  if(clights) bitSet(byteone,2);
  if(ccap) bitSet(byteone,3);
Serial.write(byteone);
Serial.write(cbat);
Serial.write(cthrot);
Serial.write(brakes);

  
}


